#include "FormationBehaviour.h"
#include "BehaviourInfo.h"
#include "BPhysicsActor.h"
#include "BPhysicsMotionState.h"
#include "ArriveBehaviour.h"
#include "InteractiveState.h"
static int index = 0;
void FormationBehaviour::CalcMotion(float* motion, BehaviourInfo* info, float weighting)
{
    if(!info->GetTargetActor())
        return;
    //transform the offset by the targets matrix
    btMatrix3x3 m;
    btVector3 x = info->GetTargetActor()->GetHeading();
    x.normalize();
    btVector3 z = info->GetTargetActor()->GetRightVector();
    btVector3 y(0,1,0);
    z.normalize();
    m[2] = z;
    m[1] = y;
    m[0] = x;//backwards
    Engine::Instance()->GetLineDrawer()->drawLine(info->GetTargetActor()->GetPosition(),
        info->GetTargetActor()->GetPosition()+x * 25, btVector3(1,0,0));
    Engine::Instance()->GetLineDrawer()->drawLine(info->GetTargetActor()->GetPosition(),
        info->GetTargetActor()->GetPosition()+y * 25, btVector3(0,1,0));
    Engine::Instance()->GetLineDrawer()->drawLine(info->GetTargetActor()->GetPosition(),
        info->GetTargetActor()->GetPosition()+z * 25, btVector3(0,0,1));
    btTransform t(m, info->GetTargetActor()->GetPosition());
    btVector3 offset = t * m_offset;

    //calculate rough time it will take to get to target
    btVector3 toTarget = info->GetTargetActor()->GetPosition() - 
                       info->GetSourceActor()->GetPosition();
    float mag = toTarget.length();
    float variable = mag / (info->GetTargetActor()->GetLinearVelocity().length() + 
                            info->GetSourceActor()->GetLinearVelocity().length());

    btVector3 targetLocation = offset + info->GetTargetActor()->GetLinearVelocity() * variable;

    //Seek to the new location 
    BehaviourI* arrive  = new ArriveBehaviour();
    info->SetTargetActor(NULL);
    info->SetTargetPosition(&targetLocation);
    arrive->CalcMotion(motion, info, 1.0f);
    Engine::Instance()->GetLineDrawer()->drawLine(info->GetSourceActor()->GetPosition(), targetLocation, btVector3(1,0,0));
    delete arrive;
}